#include "robot/robot.h"
#include "robotmap/robotmap.h"
#include "sensors/Sensor.h"
#include "sensors/Actuator.h"
#include "pluginmanager/PluginManager.h"
#include "sensors/SnowChains.h"

#include <iostream>

Robot::Robot ( const std::string& name, RobotMap* map ) :
        _map ( map ), _image ( 0 )
{
    _name = name ;
    if (_map)
        _movingActuator = new SnowChains(_map) ;
}

Robot::Robot ( std::string name ) :
        _map ( 0 ), _image ( 0 )
{
    _name = name ;
}

Robot::~Robot()
{
    for ( size_t i = 0 ; i < _sensors.size() ; i++ )
    {
        PluginManager::getInstance().destroySensor ( _sensors[i] );
    }
    for ( size_t j = 0 ; j < _actuators.size() ; j++ )
    {
        PluginManager::getInstance().destroyActuator ( _actuators[j] );
    }
    if ( _image )
        delete _image ;
}

void Robot::moveUp()
{
    _movingActuator->moveUp();
	updatesSensors() ;
}

void Robot::moveDown()
{
    _movingActuator->moveDown();
	updatesSensors() ;
}

void Robot::moveLeft()
{
    _movingActuator->moveLeft();
	updatesSensors() ;
}

void Robot::moveRight()
{
    _movingActuator->moveRight();
	updatesSensors() ;
}

QGraphicsPixmapItem* Robot::image() const
{
    return _image ;
}

const std::string& Robot::name() const
{
    return _name ;
}

int Robot::sensorsCount() const
{
    return _sensors.size() ;
}

int Robot::actuatorsCount() const
{
    return _actuators.size() ;
}

void Robot::setRobotMap ( RobotMap* robotmap )
{
    _map = robotmap ;
    if (_map)
	{
        _movingActuator = new SnowChains(_map) ;
	}
}

int Robot::addSensor ( Sensor* sensor )
{
    if ( sensor != 0 )
    {
        _sensors.push_back ( sensor ) ;
        return _sensors.size() - 1 ;
    }
    else
        return -1 ;
}

Sensor* Robot::sensor ( int id ) const
{
    if ( id >= _sensors.size() || id < 0 )
        return 0 ;
    else
        return _sensors.at ( id ) ;
}

void Robot::setImage ( const std::string& fileName )
{
    if ( _image )
        delete _image ;
    _image = new QGraphicsPixmapItem ( QPixmap::fromImage ( QImage ( fileName.c_str() ) ) ) ;
}

void Robot::setImage ( const QPixmap& pm )
{
    if ( _image )
        delete _image ;
    _image = new QGraphicsPixmapItem ( pm ) ;
}

int Robot::addActuator ( Actuator* actuator )
{
    if ( actuator != 0 )
    {
        _actuators.push_back ( actuator ) ;
        return _actuators.size() - 1 ;
    }
    else
        return -1 ;
}

Actuator* Robot::actuator ( int id ) const
{
    if ( id >= _actuators.size() || id < 0 )
        return 0 ;
    else
        return _actuators.at ( id ) ;
}

void Robot::setRobotImageZValue(int zvalue)
{
    if (_image)
        _image->setZValue(zvalue) ;
}

int Robot::sensorValue(int sens, int value) const
{
	Sensor* s = sensor(sens) ;
	if(!s)
		return -1 ;
	switch(value)
	{
		case 1 :
			return s->firstValue() ;
			break ;
		case 2 :
			return s->secondValue() ;
			break ;
		case 3 :
			return s->thirdValue() ;
			break ;
		case 4 :
			return s->fourthValue() ;
			break ;
		case 5 :
			return s->fifthValue() ;
			break ;
		default :
			return -1 ;
			break ;
	}
}

void Robot::doActuatorAction(int actu)
{
	Actuator* a = actuator(actu) ;
	if(a)
		a->doAction() ;
}

void Robot::updatesSensors()
{
	for(size_t i = 0 ; i < _sensors.size() ; i++)
		_sensors.at(i)->updateDatas() ;
	
}